#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>

#include "general.h"
#include "fifo.h"

int enqueuecount = 0;

Fifo newFifo(void)
{
  Fifo new;
  new = malloc(sizeof(Fifo_T));
  if (new == NULL)
    {
      fprintf(stderr, "newFifo() unable to allocate memory!\n");
    }
  new->head = 0;
  new->tail = 0;
  new->num = 0;
  pthread_mutex_init(&(new->lock), NULL);
  return new;
}

void delFifo(Fifo f)
{
  pthread_mutex_destroy(&(f->lock));
  free(f);
}

int isFifoEmpty(Fifo f)
{
  if (f->head == f->tail) return 1;
  else return 0;
}

void enqueue(Fifo f, FifoElement e)
{
  int tail;
  unsigned long long start, stop, lap;
  //  enqueuecount++;
  // f->num++;
  start = rdtsc();
  while (1)			/* blocking until queue'd */
    {
      /* pthread_mutex_lock(&(f->lock)); */
      /* if (incFifoPtr(f->tail) == f->head) */
      /* 	{ */
      /* 	  pthread_mutex_unlock(&(f->lock)); */
      /* 	  continue; */
      /* 	} */
      /* else */
      /* 	{ */
      /* 	  tail = f->tail; */
      /* 	  f->tail = incFifoPtr(f->tail); */
      /* 	  f->queue[tail].element = e->element; */
      /* 	  pthread_mutex_unlock(&(f->lock)); */
      /* 	  return; */
      /* 	} */


      lap = rdtsc();
      tail = f->tail;
      if (incFifoPtr(tail) == f->head) continue; /* no more space */
      if (__sync_bool_compare_and_swap(&(f->tail), tail, incFifoPtr(tail)))
      	{
      	  f->queue[tail].element = e->element;
	  stop = rdtsc();
	  //printf("l %lld\n", stop - lap);
	  //printf("s %lld\n", stop - start);
      	  return;
      	}
    }
}

void dequeue(Fifo f, FifoElement e)
{
  int head;
  unsigned long long start, stop, lap;
  // struct timespec t;
  // t.tv_sec = 0;
  //t.tv_nsec = 100;
  //static int count = 0;
  //count++;
  // start = rdtsc();
  while (1)
    {
#ifndef QUEUE
      head = f->tail;
      if (head == f->head) continue; /* nothing to dequeue */
      lap = rdtsc();
      if (__sync_bool_compare_and_swap(&(f->tail), head,
      decFifoPtr(head)))
        {
          e->element = f->queue[head].element;
          //f->queue[head].element = NULL;
          stop = rdtsc();
          //printf("l %lld\n", stop - lap);
          //printf("s %lld\n", stop - start);
          return;
        }
#else
      head = f->head;
      if (head == f->tail) continue; /* nothing to dequeue */
      lap = rdtsc();
      if (__sync_bool_compare_and_swap(&(f->head), head, incFifoPtr(head)))
      	{
      	  e->element = f->queue[head].element;
      	  //f->queue[head].element = NULL;
	  stop = rdtsc();
	  //printf("l %lld\n", stop - lap);
	  //printf("s %lld\n", stop - start);
      	  return;
      	}
#endif 
      //nanosleep(&t, NULL);
    }
}

int nbdequeue(Fifo f, FifoElement e)
{
  int head;
  while (1)
    {
      head = f->head;
      if (head == f->tail) return 0; /* nothing to dequeue */
      if (__sync_bool_compare_and_swap(&(f->head), head, incFifoPtr(head)))
	{
	  e->element = f->queue[head].element;
	  //f->queue[head].element = NULL; //dlin
	  return 1;
	}
    }
}

/* int main(int argc, char * argv[]) */
/* { */
/*   pthread_t threads[16]; */
/*   void *status; */
/*   int rc; */
/*   long t; */
/*   int i; */
/*   FifoElement_T d; */

/*   f = newFifo(); */
/*   for(t=0; t<atoi(argv[1]); t++) */
/*     { */
/*       printf("In main: creating thread %ld\n", t); */
/*       rc = pthread_create(&threads[t], NULL, tester, (void *)t); */
/*       if (rc) */
/* 	{ */
/* 	  printf("ERROR; return code from pthread_create() is %d\n", rc); */
/* 	  exit(-1); */
/* 	} */
/*     } */


/*   for(t=0; t<atoi(argv[1]); t++)  */
/*     { */
/*       rc = pthread_join(threads[t], &status); */
/*       if (rc)  */
/* 	{ */
/* 	  printf("ERROR; return code from pthread_join() is %d\n", rc); */
/* 	  exit(-1); */
/* 	} */
/*       printf("Main: completed join with thread %ld having a status of %ld\n",t,(long)status); */
/*     } */

/*   for (i = 0; i < atoi(argv[1]) * 10; i++) */
/*     { */
/*       dequeue(f, &d); */
/*       printf("%d\n", d.element); */
/*     } */

/*   pthread_exit(NULL); */
/* } */
